#include <vector>
#include<iostream>
#include <algorithm>
#include <ros/ros.h>
#include <serial/serial.h>
#include <geometry_msgs/PoseStamped.h>
#include <geographic_msgs/GeoPoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMavFrame.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <std_msgs/Float32.h>
#include <std_msgs/UInt8.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <tf/tf.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>
#include <sensor_msgs/Joy.h>
#include <sensor_msgs/Imu.h> 
#include <std_msgs/Float32MultiArray.h>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
// #include <sensor_msgs/LaserScan.h>  qaq
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/OverrideRCIn.h>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/Altitude.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
// #include <pcl/io/io.h>
// #include <pcl/io/pcd_io.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/common.h>
#include <pcl/filters/voxel_grid.h>

#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

// #include "quadrotor_msgs/PositionCommand.h"
// #include "off_board_test/gimbal.h"
template <typename T>
T limit(T a, T max)
{
  if(a > max)
    a = max;
  if(a < -max)
    a = -max;
  return a;
}

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
};

struct Point_with_id
{
	int id;
	float x;
	float y;
	float z;
	// bool existed;
	double update_time;	
};

std::vector<Point_with_id> id_list;

const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.14159265358;
int new_id = 0;
int select_id = 0;

//launch file params definition
bool simulation;
double map_size_x, map_size_y, map_size_z_min, map_size_z_max;
double inflation;
double sample_size;
double clusterTolerance; //set min R between two cloud points
int minsize; //the min number of a cluster
int maxsize; //the max number of a cluster
double min_lidar_distance;
double max_lidar_distance;
double lidar_intensity;

double track_search_field;

double lidar1_roll;
double lidar1_pitch;
double lidar1_yaw;
double lidar1_x;
double lidar1_y;
double lidar1_z;

geometry_msgs::Point current_point;
geometry_msgs::Quaternion current_angle;
Eigen::Quaterniond current_angle_Q;
geometry_msgs::Vector3 curr_angle;


//declare subscriber
ros::Subscriber lidar1_cloud_sub, select_num_sub, local_position_sub, imu_sub;

//declare publisher
ros::Publisher map1_pub, marker_pub, marker2_pub, marker_id_pub,plane_pos_pub, duoji_rate_pub;

//declare service

//declare timer events
ros::Timer cloud_pub_timer;

//lidar cloud params
pcl::PointCloud<pcl::PointXYZI> cloud1;
pcl::PointCloud<pcl::PointXYZI> cloud1_last;
pcl::PointCloud<pcl::PointXYZI> cloud1_last_last;
pcl::PointCloud<pcl::PointXYZI> cloud1_last_last_last;


//declare functions
pcl::PointCloud<pcl::PointXYZI> transform_pc2map(pcl::PointCloud<pcl::PointXYZI> latest_cloud, Eigen::Quaterniond lidar_angle_q,
                    double lidar_x, double lidar_y, double lidar_z);
visualization_msgs::Marker draw_cluster_marker(int clusterId, Box box);
visualization_msgs::Marker draw_line_marker(geometry_msgs::Vector3 vector, int length);
visualization_msgs::Marker draw_cluster_id_marker(Point_with_id id_point);
float angle_rate_p_control(float angle_error, float P, float dead_zone, float angle_rate_limit);



